// photoquat.cpp : Defines the entry point for the console application.
//

#include "ntpg.h"
#include <iostream>

using namespace std;

int main(int argc, char* argv[])
{
	uint M, N, C, U;
	M = 8;			// Number of images
	N = 12; 		// Number of object points
	C = 1;			// Number of cameras
	U = 7*M+3*C;		// Number of unkowns
	
	ntpg* solver = new ntpg(M,N,C,U);

	// for every image m set camera c = 1..C
	// there was only one camera used in this experiment
	solver->setk(1,1);	// set image 1 to camera 1
	solver->setk(2,1);  	// set image 2 to camera 1
	solver->setk(3,1);  	// set image 3 to camera 1
	solver->setk(4,1);  	// set image 4 to camera 1
	solver->setk(5,1);  	// set image 5 to camera 1
	solver->setk(6,1);  	// set image 6 to camera 1
	solver->setk(7,1);  	// set image 7 to camera 1
	solver->setk(8,1);  	// set image 8 to camera 1
	
	// for every camera c = 1..C set camera parameters
	// note: this information was obtained in the meta data from jpg file

	double magnify = 1.0;
	solver->setfocalplaneresolution(7766.99/magnify, 7741.94/magnify);
	ftype xres = solver->getXfocalplaneresolution();
	ftype yres = solver->getYfocalplaneresolution();
	solver->setv(1, 1600.0/xres*0.5, 1200.0/yres*0.5, -0.1968*magnify, 0);

	// for every image m set image position t[1..M]
	solver->sett(1, 3.0, 1.0, 24.0, 0);
	solver->sett(2, 3.0, 1.0, 24.0, 0);
	solver->sett(3, 3.0, -12.0, 18.0, 0);
	solver->sett(4, 3.0, -12.0, 18.0, 0);
	solver->sett(5, -12.0, 1.0, 18.0, 0);
	solver->sett(6, -12.0, 1.0, 18.0, 0);
	solver->sett(7, 12.0, 1.0, 18.0, 0);
	solver->sett(8, 12.0, 1.0, 18.0, 0);

	// for every image m set image orientation a[1..M]
	// use quaternion rotations (psi, theta, phi)
	solver->orientImage(1, -solver->tx(1), -solver->ty(1), -solver->tz(1), 90.0, 0);
	solver->orientImage(2, -solver->tx(2), -solver->ty(2), -solver->tz(2), 180.0, 0);
	solver->orientImage(3, -solver->tx(3), -solver->ty(3), -solver->tz(3), 0.0, 0);
	solver->orientImage(4, -solver->tx(4), -solver->ty(4), -solver->tz(4), 90.0, 0);
	solver->orientImage(5, -solver->tx(5), -solver->ty(5), -solver->tz(5), 180.0, 0);
	solver->orientImage(6, -solver->tx(6), -solver->ty(6), -solver->tz(6), 90.0, 0);
	solver->orientImage(7, -solver->tx(7), -solver->ty(7), -solver->tz(7), 90.0, 0);
	solver->orientImage(8, -solver->tx(8), -solver->ty(8), -solver->tz(8), 0.0, 0);

	
	// set object points
	solver->setr( 1, 0.0, 0.0, 0.0, 1);
	solver->setr( 2, 2.0, 0.0, 0.0, 1);
	solver->setr( 3, 4.0, 0.0, 0.0, 1);
	solver->setr( 4, 6.0, 0.0, 0.0, 1);
	solver->setr( 5, 0.0, 2.0, 0.0, 1);
	solver->setr( 6, 2.0, 2.0, 0.0, 1);
	solver->setr( 7, 4.0, 2.0, 0.0, 1);
	solver->setr( 8, 6.0, 2.0, 0.0, 1);
	solver->setr( 9, 0.0, 4.0, 0.0, 1);
	solver->setr(10, 2.0, 4.0, 0.0, 1);
	solver->setr(11, 4.0, 4.0, 0.0, 1);
	solver->setr(12, 6.0, 4.0, 0.0, 1);

	// set image points
	// set image 1 points
	solver->setu(1, 1, 424, 869, 1);
	solver->setu(1, 2, 706, 876, 1);
	solver->setu(1, 3, 994, 881, 1);
	solver->setu(1, 4, 1285, 883, 1);
	solver->setu(1, 5, 429, 585, 1);
	solver->setu(1, 6, 709, 588, 1);
	solver->setu(1, 7, 996, 590, 1);
	solver->setu(1, 8, 1284, 592, 1);
	solver->setu(1, 9, 438, 307, 1);
	solver->setu(1, 10, 714, 305, 1);
	solver->setu(1, 11, 996, 305, 1);
	solver->setu(1, 12, 1280, 307, 1);

	// set image 2 points
	solver->setu(2, 1, 1153, 980, 1);
	solver->setu(2, 2, 1156, 731, 1);
	solver->setu(2, 3, 1157, 477, 1);
	solver->setu(2, 4, 1154, 222, 1);
	solver->setu(2, 5, 901, 979, 1);
	solver->setu(2, 6, 902, 731, 1);
	solver->setu(2, 7, 901, 479, 1);
	solver->setu(2, 8, 899, 226, 1);
	solver->setu(2, 9, 654, 976, 1);
	solver->setu(2, 10, 651, 731, 1);
	solver->setu(2, 11, 649, 481, 1);
	solver->setu(2, 12, 648, 231, 1);
	
	// set image 3 points
	solver->setu(3, 1, 427, 729, 1);
	solver->setu(3, 2, 717, 727, 1);
	solver->setu(3, 3, 1009, 722, 1);
	solver->setu(3, 4, 1297, 716, 1);
	solver->setu(3, 5, 447, 473, 1);
	solver->setu(3, 6, 718, 469, 1);
	solver->setu(3, 7, 991, 465, 1);
	solver->setu(3, 8, 1260, 462, 1);
	solver->setu(3, 9, 466, 251, 1);
	solver->setu(3, 10, 719, 245, 1);
	solver->setu(3, 11, 973, 242, 1);
	solver->setu(3, 12, 1224,240, 1);
	
	// set image 4 points
	solver->setu(4, 1, 929, 983, 1);
	solver->setu(4, 2, 937, 722, 1);
	solver->setu(4, 3, 943, 457, 1);
	solver->setu(4, 4, 947, 191, 1);
	solver->setu(4, 5, 693, 954, 1);
	solver->setu(4, 6, 697, 707, 1);
	solver->setu(4, 7, 702, 457, 1);
	solver->setu(4, 8, 707, 207, 1);
	solver->setu(4, 9, 483, 926, 1);
	solver->setu(4, 10, 485, 693, 1);
	solver->setu(4, 11, 489, 458, 1);
	solver->setu(4, 12, 495, 222, 1);
	
	// set image 5 points
	solver->setu(5, 1, 1039, 773, 1);
	solver->setu(5, 2, 1036, 563, 1);
	solver->setu(5, 3, 1033, 376, 1);
	solver->setu(5, 4, 1028, 210, 1);
	solver->setu(5, 5, 802, 770, 1);
	solver->setu(5, 6, 812, 562, 1);
	solver->setu(5, 7, 822, 377, 1);
	solver->setu(5, 8, 829, 213, 1);
	solver->setu(5, 9, 572, 765, 1);
	solver->setu(5, 10, 595, 561, 1);
	solver->setu(5, 11, 616, 378, 1);
	solver->setu(5, 12, 635, 216, 1);
	
	// set image 6 points
	solver->setu(6, 1, 601, 870, 1);
	solver->setu(6, 2, 787, 846, 1);
	solver->setu(6, 3, 950, 823, 1);
	solver->setu(6, 4, 1094, 803, 1);
	solver->setu(6, 5, 576, 649, 1);
	solver->setu(6, 6, 761, 639, 1);
	solver->setu(6, 7, 924, 629, 1);
	solver->setu(6, 8, 1069, 621, 1);
	solver->setu(6, 9, 553, 435, 1);
	solver->setu(6, 10, 737, 438, 1);
	solver->setu(6, 11, 899, 440, 1);
	solver->setu(6, 12, 1043, 443, 1);
	
	// set image 7 points
	solver->setu(7, 1, 570, 775, 1);
	solver->setu(7, 2, 729, 774, 1);
	solver->setu(7, 3, 913, 773, 1);
	solver->setu(7, 4, 1127, 770, 1);
	solver->setu(7, 5, 585, 577, 1);
	solver->setu(7, 6, 741, 561, 1);
	solver->setu(7, 7, 921, 543, 1);
	solver->setu(7, 8, 1131, 523, 1);
	solver->setu(7, 9, 599, 387, 1);
	solver->setu(7, 10, 753, 358, 1);
	solver->setu(7, 11, 928, 326, 1);
	solver->setu(7, 12, 1132, 289, 1);	

	// set image 8 points
	solver->setu(8, 1, 638, 335, 1);
	solver->setu(8, 2, 652, 483, 1);
	solver->setu(8, 3, 670, 654, 1);
	solver->setu(8, 4, 691, 853, 1);
	solver->setu(8, 5, 831, 324, 1);
	solver->setu(8, 6, 860, 469, 1);
	solver->setu(8, 7, 893, 637, 1);
	solver->setu(8, 8, 931, 832, 1);
	solver->setu(8, 9, 1019, 314, 1);
	solver->setu(8, 10, 1061, 457, 1);
	solver->setu(8, 11, 1108, 620, 1);
	solver->setu(8, 12, 1162, 810, 1);

//	solver->func(solver->getp());
//	solver->dfunc(solver->getp(), solver->getg());
/*
	for(uint i=1; i<=solver->getT(); i++) {
		cout << i << '\t' << solver->getp(i) << endl;
	}
//*/
/*
	for(uint n=1; n<=solver->getN(); n++) {
		cout << "r[" << n << "] = <" << solver->rx(n) << ", " << solver->ry(n) << ", " << solver->rz(n) << ">" << endl;
	}
//*/
/*
	for(uint m=1; m<=solver->getM(); m++) {
		cout << "t[" << m << "] = <" << solver->tx(m) << ", " << solver->ty(m) << ", " << solver->tz(m) << ">" << endl;
	}
//*/
/*
	for(uint m=1; m<=solver->getM(); m++) {
		cout << "a[" << m << "] = <" << solver->ax(m) << ", " << solver->ay(m) << ", " << solver->az(m) << ">" << endl;
	}
//*/
/*
	for(uint m=1; m<=solver->getM(); m++) {
		cout << "v[" << m << "] = <" << solver->vx(m) << ", " << solver->vy(m) << ", " << solver->vz(m) << ">" << endl;
	}
//*/
/*
	for(uint n=1; n<=solver->getN(); n++) {
		cout << "n=" << n << '\t';
		for(uint m=1; m<=solver->getM(); m++) {
			cout << "<" << solver->ux(m,n)*7766.99<< ", " << solver->uy(m,n)*7741.94 << ">"  << '\t';
		}
		cout << endl;
	}
//*/

//	simplex optimizer
	solver->solve(1.0e-10);	

//	float rx, ry, rz;
//	rx = solver->rx(2);
//	ry = solver->ry(2);
//	rz = solver->rz(2);
//	cout << "r(" << 2 << ") = [" << rx << "," << ry << "," << rz << "]" << " |r| = " << sqrt(rx*rx+ry*ry+rz*rz) << endl;


/*	
	cout << "initial error = " << solver->func(solver->getp()) << endl;
	float vx, vy, vz;
	vx = solver->vx(1);
	vy = solver->vy(1);
	vz = solver->vz(1);
	

	solver->solve(1.0e-14);

	cout << "v(" << 1 << ") = [" << vx << "," << vy << "," << vz << "]" << " |v| = " << sqrt(vx*vx+vy*vy+vz*vz) << endl;
	vx = solver->vx(1);
	vy = solver->vy(1);
	vz = solver->vz(1);
	cout << "v(" << 1 << ") = [" << vx << "," << vy << "," << vz << "]" << " |v| = " << sqrt(vx*vx+vy*vy+vz*vz) << endl;

	uint m = 1;
	float tx, ty, tz;
	for(m=1; m<=solver->getM(); m++) {
		tx = solver->tx(m);
		ty = solver->ty(m);
		tz = solver->tz(m);
		cout << "t(" << m << ") = [" << tx << "," << ty << "," << tz << "]" << " |t| = " << sqrt(tx*tx+ty*ty+tz*tz) << endl;
	}
//*/

	delete solver;
	return 0;
}

